;/***************************************************************************
; * Copyright (c) 2024 Microsoft Corporation 
; * 
; * This program and the accompanying materials are made available under the
; * terms of the MIT License which is available at
; * https://opensource.org/licenses/MIT.
; * 
; * SPDX-License-Identifier: MIT
; **************************************************************************/
;
;
;/**************************************************************************/
;/**************************************************************************/
;/**                                                                       */
;/** ThreadX Component                                                     */
;/**                                                                       */
;/**   Thread - Low Level SMP Support                                      */
;/**                                                                       */
;/**************************************************************************/
;/**************************************************************************/

    MACRO
$label _tx_thread_smp_protect_lock_got
;
;    /* Set the currently owned core.  */
;    _tx_thread_smp_protection.tx_thread_smp_protect_core = this_core;
;
    STR     r1, [r2, #8]                        ; Store this core
;
;    /* Increment the protection count. */
;    _tx_thread_smp_protection.tx_thread_smp_protect_count++;
;
    LDR     r3, [r2, #12]                       ; Pickup ownership count
    ADD     r3, r3, #1                          ; Increment ownership count
    STR     r3, [r2, #12]                       ; Store ownership count
    DMB

    IF :DEF:TX_MPCORE_DEBUG_ENABLE
    LSL     r3, r1, #2                          ; Build offset to array indexes
    LDR     r4, =_tx_thread_current_ptr         ; Pickup start of the current thread array
    ADD     r4, r3, r4                          ; Build index into the current thread array
    LDR     r3, [r4]                            ; Pickup current thread for this core
    STR     r3, [r2, #4]                        ; Save current thread pointer
    STR     LR, [r2, #16]                       ; Save caller's return address
    STR     r0, [r2, #20]                       ; Save CPSR
    ENDIF

    MEND

    MACRO
$label _tx_thread_smp_protect_remove_from_front_of_list
;
;    /* Remove ourselves from the list.  */
;    _tx_thread_smp_protect_wait_list[_tx_thread_smp_protect_wait_list_head++] =  0xFFFFFFFF;
;
    MOV     r3, #0xFFFFFFFF                     ; Build the invalid core value
    LDR     r4, =_tx_thread_smp_protect_wait_list_head ; Get the address of the head
    LDR     r5, [r4]                            ; Get the value of the head
    LDR     r6, =_tx_thread_smp_protect_wait_list ; Get the address of the list
    STR     r3, [r6, r5, LSL #2]                ; Store the invalid core value
    ADD     r5, r5, #1                          ; Increment the head
;
;    /* Did we wrap?  */
;    if (_tx_thread_smp_protect_wait_list_head == TX_THREAD_SMP_MAX_CORES + 1)
;    {
;
    LDR     r3, =_tx_thread_smp_protect_wait_list_size ; Load address of core list size
    LDR     r3, [r3]                            ; Load the max cores value
    CMP     r5, r3                              ; Compare the head to it
    BNE     $label._store_new_head              ; Are we at the max?
;
;    _tx_thread_smp_protect_wait_list_head = 0;
;
    EOR     r5, r5, r5                          ; We're at the max. Set it to zero
;
;    }
;
$label._store_new_head

    STR     r5, [r4]                            ; Store the new head
;
;    /* We have the lock!  */
;    return;
;
    MEND


    MACRO
$label _tx_thread_smp_protect_wait_list_lock_get
;VOID  _tx_thread_smp_protect_wait_list_lock_get()
;{
;    /* We do this until we have the lock.  */
;    while (1)
;    {
;
$label._tx_thread_smp_protect_wait_list_lock_get__try_to_get_lock
;
;    /* Is the list lock available?  */
;    _tx_thread_smp_protect_wait_list_lock_protect_in_force = load_exclusive(&_tx_thread_smp_protect_wait_list_lock_protect_in_force);
;
    LDR     r1, =_tx_thread_smp_protect_wait_list_lock_protect_in_force
    LDREX   r2, [r1]                            ; Pickup the protection flag
;
;    if (protect_in_force == 0)
;    {
;
    CMP     r2, #0
    BNE     $label._tx_thread_smp_protect_wait_list_lock_get__try_to_get_lock ; No, protection not available
;
;    /* Try to get the list.  */
;    int status = store_exclusive(&_tx_thread_smp_protect_wait_list_lock_protect_in_force, 1);
;
    MOV     r2, #1                              ; Build lock value
    STREX   r3, r2, [r1]                        ; Attempt to get the protection
;
;    if (status == SUCCESS)
;
    CMP     r3, #0
    BNE     $label._tx_thread_smp_protect_wait_list_lock_get__try_to_get_lock ; Did it fail? If so, try again.
;
;    /* We have the lock!  */
;    return;
;
    MEND


    MACRO
$label _tx_thread_smp_protect_wait_list_add
;VOID  _tx_thread_smp_protect_wait_list_add(UINT new_core)
;{
;
;    /* We're about to modify the list, so get the list lock.  */
;    _tx_thread_smp_protect_wait_list_lock_get();
;
    PUSH    {r1-r2}

$label _tx_thread_smp_protect_wait_list_lock_get

    POP     {r1-r2}
;
;    /* Add this core.  */
;    _tx_thread_smp_protect_wait_list[_tx_thread_smp_protect_wait_list_tail++] = new_core;
;
    LDR     r3, =_tx_thread_smp_protect_wait_list_tail ; Get the address of the tail
    LDR     r4, [r3]                            ; Get the value of tail
    LDR     r5, =_tx_thread_smp_protect_wait_list ; Get the address of the list
    STR     r1, [r5, r4, LSL #2]                ; Store the new core value
    ADD     r4, r4, #1                          ; Increment the tail
;
;    /* Did we wrap?  */
;    if (_tx_thread_smp_protect_wait_list_tail == _tx_thread_smp_protect_wait_list_size)
;    {
;
    LDR     r5, =_tx_thread_smp_protect_wait_list_size ; Load max cores address
    LDR     r5, [r5]                            ; Load max cores value
    CMP     r4, r5                              ; Compare max cores to tail
    BNE     $label._tx_thread_smp_protect_wait_list_add__no_wrap ; Did we wrap?
;
;    _tx_thread_smp_protect_wait_list_tail = 0;
;
    MOV     r4, #0
;
;    }
;
$label._tx_thread_smp_protect_wait_list_add__no_wrap

    STR     r4, [r3]                            ; Store the new tail value.
;
;    /* Release the list lock.  */
;    _tx_thread_smp_protect_wait_list_lock_protect_in_force = 0;
;
    MOV     r3, #0                              ; Build lock value
    LDR     r4, =_tx_thread_smp_protect_wait_list_lock_protect_in_force
    STR     r3, [r4]                            ; Store the new value

    MEND


    MACRO
$label _tx_thread_smp_protect_wait_list_remove
;VOID _tx_thread_smp_protect_wait_list_remove(UINT core)
;{
;
;    /* Get the core index.  */
;    UINT core_index;
;    for (core_index = 0;; core_index++)
;
    EOR     r1, r1, r1                          ; Clear for 'core_index'
    LDR     r2, =_tx_thread_smp_protect_wait_list ; Get the address of the list
;
;    {
;
$label._tx_thread_smp_protect_wait_list_remove__check_cur_core
;
;    /* Is this the core?  */
;    if (_tx_thread_smp_protect_wait_list[core_index] == core)
;    {
;        break;
;
    LDR     r3, [r2, r1, LSL #2]                ; Get the value at the current index
    CMP     r3, r10                             ; Did we find the core?
    BEQ     $label._tx_thread_smp_protect_wait_list_remove__found_core
;
;    }
;
    ADD     r1, r1, #1                          ; Increment cur index
    B       $label._tx_thread_smp_protect_wait_list_remove__check_cur_core ; Restart the loop
;
;    }
;
$label._tx_thread_smp_protect_wait_list_remove__found_core
;
;    /* We're about to modify the list. Get the lock. We need the lock because another
;       core could be simultaneously adding (a core is simultaneously trying to get
;       the inter-core lock) or removing (a core is simultaneously being preempted,
;       like what is currently happening).  */
;    _tx_thread_smp_protect_wait_list_lock_get();
;
    PUSH    {r1}

$label _tx_thread_smp_protect_wait_list_lock_get

    POP     {r1}
;
;    /* We remove by shifting.  */
;    while (core_index != _tx_thread_smp_protect_wait_list_tail)
;    {
;
$label._tx_thread_smp_protect_wait_list_remove__compare_index_to_tail

    LDR     r2, =_tx_thread_smp_protect_wait_list_tail ; Load tail address
    LDR     r2, [r2]                            ; Load tail value
    CMP     r1, r2                              ; Compare cur index and tail
    BEQ     $label._tx_thread_smp_protect_wait_list_remove__removed
;
;    UINT next_index = core_index + 1;
;
    MOV     r2, r1                              ; Move current index to next index register
    ADD     r2, r2, #1                          ; Add 1
;
;    if (next_index == _tx_thread_smp_protect_wait_list_size)
;    {
;
    LDR     r3, =_tx_thread_smp_protect_wait_list_size
    LDR     r3, [r3]
    CMP     r2, r3
    BNE     $label._tx_thread_smp_protect_wait_list_remove__next_index_no_wrap
;
;    next_index = 0;
;
    MOV     r2, #0
;
;    }
;
$label._tx_thread_smp_protect_wait_list_remove__next_index_no_wrap
;
;    list_cores[core_index] = list_cores[next_index];
;
    LDR     r0, =_tx_thread_smp_protect_wait_list ; Get the address of the list
    LDR     r3, [r0, r2, LSL #2]                ; Get the value at the next index
    STR     r3, [r0, r1, LSL #2]                ; Store the value at the current index
;
;    core_index = next_index;
;
    MOV     r1, r2

    B       $label._tx_thread_smp_protect_wait_list_remove__compare_index_to_tail
;
;    }
;
$label._tx_thread_smp_protect_wait_list_remove__removed
;
;    /* Now update the tail.  */
;    if (_tx_thread_smp_protect_wait_list_tail == 0)
;    {
;
    LDR     r0, =_tx_thread_smp_protect_wait_list_tail ; Load tail address
    LDR     r1, [r0]                            ; Load tail value
    CMP     r1, #0
    BNE     $label._tx_thread_smp_protect_wait_list_remove__tail_not_zero
;
;    _tx_thread_smp_protect_wait_list_tail = _tx_thread_smp_protect_wait_list_size;
;
    LDR     r2, =_tx_thread_smp_protect_wait_list_size
    LDR     r1, [r2]
;
;    }
;
$label._tx_thread_smp_protect_wait_list_remove__tail_not_zero
;
;    _tx_thread_smp_protect_wait_list_tail--;
;
    SUB     r1, r1, #1
    STR     r1, [r0]                            ; Store new tail value
;
;    /* Release the list lock.  */
;    _tx_thread_smp_protect_wait_list_lock_protect_in_force = 0;
;
    MOV     r0, #0                              ; Build lock value
    LDR     r1, =_tx_thread_smp_protect_wait_list_lock_protect_in_force ; Load lock address
    STR     r0, [r1]                            ; Store the new value
;
;    /* We're no longer waiting. Note that this should be zero since, again,
;       this function is only called when a thread preemption is occurring.  */
;    _tx_thread_smp_protect_wait_counts[core]--;
;
    LDR     r1, =_tx_thread_smp_protect_wait_counts ; Load wait list counts
    LDR     r2, [r1, r10, LSL #2]               ; Load waiting value
    SUB     r2, r2, #1                          ; Subtract 1
    STR     r2, [r1, r10, LSL #2]               ; Store new waiting value
    MEND

